#include "XMath.h"

#include "lib/memory/pvmmemory.h"

using namespace pvmlib;

//aabbox3d aabbox3d::emptyBox(SCALAR_ZERO, SCALAR_ZERO, SCALAR_ZERO,  SCALAR_ZERO, SCALAR_ZERO, SCALAR_ZERO);
/*
Matrix Matrix::identity(true);

//=============================================================== Matrix
Matrix Matrix::operator * (const Matrix& B) const
{
	Matrix C;

#if defined(_WIN32_WCE) && !defined(SCALAR_FIXEDPOINT) &&  defined(PVM_USE_MATH_LIB)
	MultMatrix((float*)C, (float*)this, (float*)B);
#else
	C.a00 = B.a00 * a00 + B.a01 * a10 + B.a02 * a20 + B.a03 * a30;
	C.a10 = B.a10 * a00 + B.a11 * a10 + B.a12 * a20 + B.a13 * a30;
	C.a20 = B.a20 * a00 + B.a21 * a10 + B.a22 * a20 + B.a23 * a30;
	C.a30 = B.a30 * a00 + B.a31 * a10 + B.a32 * a20 + B.a33 * a30;
	C.a01 = B.a00 * a01 + B.a01 * a11 + B.a02 * a21 + B.a03 * a31;
	C.a11 = B.a10 * a01 + B.a11 * a11 + B.a12 * a21 + B.a13 * a31;
	C.a21 = B.a20 * a01 + B.a21 * a11 + B.a22 * a21 + B.a23 * a31;
	C.a31 = B.a30 * a01 + B.a31 * a11 + B.a32 * a21 + B.a33 * a31;
	C.a02 = B.a00 * a02 + B.a01 * a12 + B.a02 * a22 + B.a03 * a32;
	C.a12 = B.a10 * a02 + B.a11 * a12 + B.a12 * a22 + B.a13 * a32;
	C.a22 = B.a20 * a02 + B.a21 * a12 + B.a22 * a22 + B.a23 * a32;
	C.a32 = B.a30 * a02 + B.a31 * a12 + B.a32 * a22 + B.a33 * a32;
	C.a03 = B.a00 * a03 + B.a01 * a13 + B.a02 * a23 + B.a03 * a33;
	C.a13 = B.a10 * a03 + B.a11 * a13 + B.a12 * a23 + B.a13 * a33;
	C.a23 = B.a20 * a03 + B.a21 * a13 + B.a22 * a23 + B.a23 * a33;
	C.a33 = B.a30 * a03 + B.a31 * a13 + B.a32 * a23 + B.a33 * a33;
#endif
	return C;
}

scalar Matrix::det3x3(const scalar a1, const scalar a2, const scalar a3,
			 const scalar b1, const scalar b2, const scalar b3,
			 const scalar c1, const scalar c2, const scalar c3)
{
	return a1 * det2x2(b2, b3, c2, c3) - b1 * det2x2(a2, a3, c2, c3) + c1 * det2x2(a2, a3, b2, b3);
}

void Matrix::Invert()
{
	scalar det, oodet;

	Matrix B;

	B.a00 =  det3x3(a11, a21, a31, a12, a22, a32, a13, a23, a33);
	B.a10 = -det3x3(a10, a20, a30, a12, a22, a32, a13, a23, a33);
	B.a20 =  det3x3(a10, a20, a30, a11, a21, a31, a13, a23, a33);
	B.a30 = -det3x3(a10, a20, a30, a11, a21, a31, a12, a22, a32);

	B.a01 = -det3x3(a01, a21, a31, a02, a22, a32, a03, a23, a33);
	B.a11 =  det3x3(a00, a20, a30, a02, a22, a32, a03, a23, a33);
	B.a21 = -det3x3(a00, a20, a30, a01, a21, a31, a03, a23, a33);
	B.a31 =  det3x3(a00, a20, a30, a01, a21, a31, a02, a22, a32);

	B.a02 =  det3x3(a01, a11, a31, a02, a12, a32, a03, a13, a33);
	B.a12 = -det3x3(a00, a10, a30, a02, a12, a32, a03, a13, a33);
	B.a22 =  det3x3(a00, a10, a30, a01, a11, a31, a03, a13, a33);
	B.a32 = -det3x3(a00, a10, a30, a01, a11, a31, a02, a12, a32);

	B.a03 = -det3x3(a01, a11, a21, a02, a12, a22, a03, a13, a23);
	B.a13 =  det3x3(a00, a10, a20, a02, a12, a22, a03, a13, a23);
	B.a23 = -det3x3(a00, a10, a20, a01, a11, a21, a03, a13, a23);
	B.a33 =  det3x3(a00, a10, a20, a01, a11, a21, a02, a12, a22);

	det = (a00 * B.a00) + (a01 * B.a10) + (a02 * B.a20) + (a03 * B.a30);

	oodet = SCALAR_ONE / det;

	B.a00 *= oodet;
	B.a10 *= oodet;
	B.a20 *= oodet;
	B.a30 *= oodet;

	B.a01 *= oodet;
	B.a11 *= oodet;
	B.a21 *= oodet;
	B.a31 *= oodet;

	B.a02 *= oodet;
	B.a12 *= oodet;
	B.a22 *= oodet;
	B.a32 *= oodet;

	B.a03 *= oodet;
	B.a13 *= oodet;
	B.a23 *= oodet;
	B.a33 *= oodet;

	*this = B;
}

void Matrix::Invert3()
{
	scalar det, oodet;
	Matrix B;
	B.Identity();

	B.a00 =  (a11 * a22 - a21 * a12);
	B.a10 = -(a10 * a22 - a20 * a12);
	B.a20 =  (a10 * a21 - a20 * a11);
	B.a01 = -(a01 * a22 - a21 * a02);
	B.a11 =  (a00 * a22 - a20 * a02);
	B.a21 = -(a00 * a21 - a20 * a01);
	B.a02 =  (a01 * a12 - a11 * a02);
	B.a12 = -(a00 * a12 - a10 * a02);
	B.a22 =  (a00 * a11 - a10 * a01);

	det = (a00 * B.a00) + (a01 * B.a10) + (a02 * B.a20);

	oodet = SCALAR_ONE / det;

	B.a00 *= oodet; B.a01 *= oodet; B.a02 *= oodet;
	B.a10 *= oodet; B.a11 *= oodet; B.a12 *= oodet;
	B.a20 *= oodet; B.a21 *= oodet; B.a22 *= oodet;
	*this = B;
}

void Matrix::LookAt(const Vector& eye, const Vector& target, const Vector& up)
{
	Vector x, y, z;

	// Z vector
	z = eye - target;
	z.normalize();

	// Y vector
	y = up;

	// X vector = Y cross Z
	x = y.crossProduct(z);
	x.normalize();

	// Recompute Y = Z cross X
	y = z.crossProduct(x);
	y.normalize();

	a00 = x.x; 
	a01 = x.y; 
	a02 = x.z; 	
	a03 = -x.dotProduct(eye);

	a10 = y.x; 
	a11 = y.y; 
	a12 = y.z; 
	a13 = -y.dotProduct(eye);

	a20 = z.x; 
	a21 = z.y; 
	a22 = z.z; 
	a23 = -z.dotProduct(eye);

	a30 = SCALAR_ZERO; 
	a31 = SCALAR_ZERO; 
	a32 = SCALAR_ZERO; 
	a33 = SCALAR_ONE;
}

void Matrix::Ortho(const scalar l, const scalar r, const scalar b, const scalar t, const scalar n, const scalar f)
{
	Reset();
	scalar invRL = SCALAR_ONE / (r - l);
	scalar invTB = SCALAR_ONE / (t - b);
	scalar invFN = SCALAR_ONE / (f - n);

	a00 = SCALAR_TWO * invRL;
	a11 = SCALAR_TWO * invTB;
	a22 = -SCALAR_TWO * invFN;
	a33 = SCALAR_ONE;

	a03 = -(r + l) * invRL;
	a13 = -(t + b) * invTB;
	a23 = -(f + n) * invFN;
}

void Matrix::Frustum(const scalar l, const scalar r, const scalar b, const scalar t, const scalar n, const scalar f)
{
	a00 = (SCALAR_TWO * n) / (r - l);
	a10 = SCALAR_ZERO;
	a20 = SCALAR_ZERO;
	a30 = SCALAR_ZERO;

	a01 = SCALAR_ZERO;
	a11 = (SCALAR_TWO*n) / (t - b);
	a21 = SCALAR_ZERO;
	a31 = SCALAR_ZERO;

	a02 = (r+l) / (r-l);
	a12 = (t+b) / (t-b);
	a22 = -(f+n) / (f-n);
	a32 = -SCALAR_ONE;

	a03 = SCALAR_ZERO;
	a13 = SCALAR_ZERO;
	a23 = -(SCALAR_TWO * f * n) / (f - n);
	a33 = SCALAR_ZERO;
}

void Matrix::Perspective(const scalar fovy, const scalar aspect, const scalar n, const scalar f)
{
	//scalar xmin, xmax, ymin, ymax;

	//ymax = n * tans(fovy * SCALAR_HALF);
	//ymin = -ymax;

	//xmin = ymin * aspect;
	//xmax = ymax * aspect;

	//Frustum(xmin, xmax, ymin, ymax, n, f);

	scalar k = SCALAR_ONE / tans(fovy * SCALAR_HALF);

	a00 = k / aspect;
	a10 = SCALAR_ZERO;
	a20 = SCALAR_ZERO;
	a30 = SCALAR_ZERO;

	a01 = SCALAR_ZERO;
	a11 = k;
	a21 = SCALAR_ZERO;
	a31 = SCALAR_ZERO;

	a02 = SCALAR_ZERO;
	a12 = SCALAR_ZERO;
	a22 = (f+n) / (n-f);
	a32 = -SCALAR_ONE;

	a03 = SCALAR_ZERO;
	a13 = SCALAR_ZERO;
	a23 = (SCALAR_TWO * f * n) / (n - f);
	a33 = SCALAR_ZERO;
}

Vector Matrix::GetTranslateVector() const
{
	return Vector(x,y,z);
}
void Matrix::Scale(const scalar sx, const scalar sy, const scalar sz)
{
	Matrix m;

	m.Identity();
	m.a00 = sx;
	m.a11 = sy;
	m.a22 = sz;

	*this *= m;
}
Vector Matrix::GetScaleVector() const
{
	return Vector(a00 / a33, a11 / a33, a22 / a33);
}
*/
//=============================================================== Vector
/*Vector& Vector::operator*=(const Matrix &M)
{
	Vector u;
	
	u.x = M.a00 * x + M.a01 * y + M.a02 * z + M.a03;
	u.y = M.a10 * x + M.a11 * y + M.a12 * z + M.a13;
	u.z = M.a20 * x + M.a21 * y + M.a22 * z + M.a23;
	*this = u;

	return *this;
}

Matrix Vector::getScaleMatrix() const
{
	Matrix matrix;

	matrix.Identity();
	matrix.a00=x;
	matrix.a11=y;
	matrix.a22=z;

	return matrix;
}

Matrix Vector::getTranslateMatrix() const
{
	Matrix matrix;

	matrix.Identity();
	matrix.a03=x;
	matrix.a13=y;
	matrix.a23=z;

	return matrix;
}
*/
scalar Vector::cosine(const Vector& vector2) const
{
	scalar dot = dotProduct(vector2);
	dot /= getLength();
	dot /= vector2.getLength();
	return dot;
}

scalar Vector::cosineForNormalized(const Vector& vector2) const
{
	return dotProduct(vector2);
}

Vector Vector::crossProduct(const Vector& w) const
{
	Vector u;
	u.x = y*w.z - z*w.y;
	u.y = z*w.x - x*w.z;
	u.z = x*w.y - y*w.x;
	return u;
}



void Vector::scale(const scalar sx, const scalar sy, const scalar sz, const Vector *axis/* = NULL*/)
{
	if (axis)
	{
		x = (x - axis->x)*sx + axis->x;
		y = (y - axis->y)*sy + axis->y;
		z = (z - axis->z)*sz + axis->z;
	}
	else
	{
		x *= sx;
		y *= sy;
		z *= sz;
	}
}

/*Vector& Vector::transform3(const Matrix &M)
{
	Vector u;

	u.x = M.a00 * x + M.a01 * y + M.a02 * z;
	u.y = M.a10 * x + M.a11 * y + M.a12 * z;
	u.z = M.a20 * x + M.a21 * y + M.a22 * z;
	*this = u;

	return *this;
}

Vector& Vector::transform(const Matrix &M)
{
	Vector u;

	u.x = M.a00 * x + M.a01 * y + M.a02 * z + M.a03;
	u.y = M.a10 * x + M.a11 * y + M.a12 * z + M.a13;
	u.z = M.a20 * x + M.a21 * y + M.a22 * z + M.a23;
	*this = u;

	return *this;
}

void Vector::project(const Matrix &modelview, const Matrix &projection, const scalar vp_x, const scalar vp_y, const scalar vp_width, const scalar vp_height, const scalar near_clip, const scalar far_clip)
{
	scalar vp_width2 = vp_width * SCALAR_HALF;
	scalar vp_height2 = vp_height * SCALAR_HALF;
	
	Vector pr(*this);
	pr *= modelview;
	scalar w = projection.a30*pr.x + projection.a31*pr.y + projection.a32*pr.z + projection.a33;
	pr *= projection;
	pr /= w;

	x = vp_width2*pr.x + vp_x + vp_width2;
	y = vp_height2*pr.y + vp_y + vp_height2;
	z = ((far_clip-near_clip) * SCALAR_HALF) * pr.z + (near_clip+far_clip) * SCALAR_HALF;
}
*/
void Vector::rotate(const Quat &quat)
{
	Quat quatInv(quat), quatRes;
	quatInv.Invert();

	quatRes.Set(x, y, z, SCALAR_ZERO);
	quatRes = quat * quatRes * quatInv;

	x = quatRes.x;
	y = quatRes.y;
	z = quatRes.z;
}

//=============================================================== Quat
void Quat::FromTo(const Vector &from, const Vector& to)
{
	Vector axis = from.crossProduct(to);
	scalar cosAngle = from.cosine(to);
    FromAngAxis(acoss(cosAngle), axis);
/*
	// Based on Stan Melax's article in Game Programming Gems
	Vector v0 = from;
	Vector v1 = to;
	v0.normalize();
	v1.normalize();

	Vector c = v0.crossProduct(v1);

	scalar d = v0.dotProduct(v1);
	if (d >= SCALAR_ONE) // If dot == 1, vectors are the same
	{
		Identity();
		return;
	}

	scalar s = sqrts((SCALAR_ONE + d) * SCALAR_TWO);
	scalar invs = SCALAR_ONE / s;

	x = c.x * invs;
	y = c.y * invs;
	z = c.z * invs;
	w = s * SCALAR_HALF;
*/
}

scalar Quat::FromToByAngle(const Vector &from, const Vector& to, const scalar angle)
{
	Vector v0 = from;
	Vector v1 = to;
	v0.normalize();
	v1.normalize();

	scalar d = v0.dotProduct(v1);
	if (d >= SCALAR_ONE)
	{
		Identity();
		return SCALAR_ZERO;
	}

	scalar resAngle = acoss(d);
	resAngle = (angle < resAngle? angle: resAngle);

	Vector axis = v0.crossProduct(v1);
	FromAngAxis(resAngle, axis);
	
	return resAngle;
}
/*
void Quat::FromLookAt(const Vector &view, const Vector &up)
{
	Vector xup(up);
	Quat q;
	q.FromTo(Vector(SCALAR_ZERO, SCALAR_ZERO, -SCALAR_ONE), view);

	Matrix tm = q.GetMatrix();
	tm.Invert3();
	xup.transform3(tm);
	Quat q2;
	q2.FromTo(Vector(SCALAR_ZERO, SCALAR_ONE, SCALAR_ZERO), xup);
	Quat q3 = q * q2;
	*this = q3;
}
*/
void Quat::FromAngAxis(const scalar ang, const Vector &axis)
{
	if (fabss(ang) < SCALAR_ALMOST_ZERO) 
	{
		w = SCALAR_ONE;
		x = y = z = SCALAR_ZERO;
		return;
	}
	Vector tmp(axis);
	tmp.normalize();

	scalar ang2 = ang * SCALAR_HALF;
	scalar scale = sins(ang2);
	x = tmp.x*scale;
	y = tmp.y*scale;
	z = tmp.z*scale;

	w = coss(ang2);
}

/*void Quat::FromMatrix(const Matrix& m)
{
	scalar tr = m.a00 + m.a11 + m.a22 + SCALAR_ONE; // trace of martix
	if (tr > SCALAR_ALMOST_ZERO)
	{     
		scalar s = sqrts(tr) * SCALAR_TWO;
		scalar invS = SCALAR_ONE / s;
		// if trace positive than "w" is biggest component
		x = (m.a12 - m.a21) * invS;
		y = (m.a20 - m.a02) * invS;
		z = (m.a01 - m.a10) * invS;
		w = SCALAR_QUARTER * s;
	}
	else if(m.a00 > m.a11 && m.a00 > m.a22) // Some of vector components is bigger
	{
		// a00 is the greatest
		scalar s = sqrts(SCALAR_ONE + m.a00 - m.a11 - m.a22) * SCALAR_TWO;
		scalar invS = SCALAR_ONE / s;
		x = SCALAR_QUARTER * s;
		y = (m.a01 + m.a10) * invS;
		z = (m.a02 + m.a20) * invS;
		w = (m.a12 - m.a21) * invS;
	}
	else if (m.a11 > m.a22)
	{
		// a11 is the greatest
		scalar s = sqrts(SCALAR_ONE + m.a11 - m.a00 - m.a22) * SCALAR_TWO;
		scalar invS = SCALAR_ONE / s;
		x = (m.a01 + m.a10) * invS;
		y = SCALAR_QUARTER * s;
		z = (m.a12 + m.a21) * invS;
		w = (m.a20 - m.a02) * invS;
	}
	else
	{
		// a22 is the greatest
		scalar s = sqrts(SCALAR_ONE + m.a22 - m.a00 - m.a11) * SCALAR_TWO;
		scalar invS = SCALAR_ONE / s;
		x = (m.a02 + m.a20) * invS;
		y = (m.a12 + m.a21) * invS;
		z = SCALAR_QUARTER * s;
		w = (m.a01 - m.a10) * invS;
	}
}
*/
void Quat::ToAngleAxis(scalar& ang, Vector& axis) const
{
	Quat q(*this);
	if (q.w > SCALAR_ONE)
	{
		q.Normalize();
	}

	ang = acoss(q.w) * SCALAR_TWO;
	scalar sinAngle = sqrts(SCALAR_ONE - q.w * q.w);
	if (fabss(sinAngle) < SCALAR_ALMOST_ZERO)
	{
		axis.set(q.x, q.y, q.z);
	}
	else
	{
		scalar inv = SCALAR_ONE / sinAngle;
		axis.set(q.x * inv, q.y * inv, q.z * inv);
	}
}

/*Matrix Quat::GetMatrix() const
{
	Matrix matrix;
	matrix.Identity();
	
	scalar w2 = w * w * SCALAR_TWO;
	scalar x2 = x * x * SCALAR_TWO;
	scalar y2 = y * y * SCALAR_TWO;
	scalar z2 = z * z * SCALAR_TWO;

	matrix.a00 = SCALAR_ONE - (y2 + z2);
	matrix.a11 = SCALAR_ONE - (x2 + z2);
	matrix.a22 = SCALAR_ONE - (x2 + y2);

	scalar wz = w*z, xy = x*y;        
	matrix.a01 = SCALAR_TWO * (xy - wz);
	matrix.a10 = SCALAR_TWO * (xy + wz);

	scalar wx = w*x, yz = y*z;
	matrix.a12 = SCALAR_TWO * (yz - wx);
	matrix.a21 = SCALAR_TWO * (yz + wx);

	scalar wy = w*y, xz = x*z;
	matrix.a02 = SCALAR_TWO * (xz + wy);
	matrix.a20 = SCALAR_TWO * (xz - wy);

	return matrix;
}
*/
void Quat::Rotate(const scalar ang, const scalar ax, const scalar ay, const scalar az)
{
	Vector axis;

	axis.set(ax,ay,az);
	Quat q1;
	q1.FromAngAxis(ang,axis);

	*this *= q1;
}
/*
Vector Quat::LinearVelocity(const Vector& point) const
{
	Vector a(SCALAR_ONE, SCALAR_ZERO, SCALAR_ZERO);
	Vector b = a;
	b.transform(this->GetMatrix());
	Vector bx(b.x, SCALAR_ZERO, SCALAR_ZERO), by(SCALAR_ZERO, b.y, SCALAR_ZERO), bz(SCALAR_ZERO, SCALAR_ZERO, b.z);

	scalar ax = acoss(a.cosine(bx));
	scalar ay = acoss(a.cosine(by));
	scalar az = acoss(a.cosine(bz));
	scalar r = point.getLength();
	Vector res(ax*r, ay*r, az*r);
	return res;
}
*/
Quat Quat::Slerp(const Quat& q2, const scalar t) const
{
	scalar a, k1, k2;
	scalar eps = SCALAR_ALMOST_ZERO;

	scalar cosvalue = x*q2.x + y*q2.y + z*q2.z + w*q2.w;
	if (cosvalue <= -SCALAR_ONE || cosvalue >= SCALAR_ONE)
	{
		cosvalue = SCALAR_ONE;
	}

	a = acoss(cosvalue);

	if (a < eps && a > -eps) 
	{
		//a == 0
		k2 = t;
		k1 = SCALAR_ONE - t;
		Quat xq2(q2);
		return (*this)*k1 + xq2*k2;
	}

	k2 = sins(t*a) / sins(a);
	k1 = sins((SCALAR_ONE - t) * a) / sins(a);
	Quat xq2(q2);

	return (*this)*k1 + xq2*k2;
}
